package model.robot;

import model.valuemap.ValueMap;
import model.virtualmap.OccupancyMap;

import java.io.PipedInputStream;
import java.io.BufferedReader;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.io.PipedOutputStream;
import java.io.IOException;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;
import java.util.StringTokenizer;

/**
 * Title    :   The Mobile Robot Explorer Simulation Environment v2.0
 * Copyright:   GNU General Public License as published by the Free Software Foundation
 * Company  :   Hanze University of Applied Sciences
 *
 * @author Dustin Meijer        (2012)
 * @author Alexander Jeurissen  (2012)
 * @author Davide Brugali       (2002)
 * @version 2.0
 */

public class MobileRobotAIgrid implements Runnable {

	private final OccupancyMap map;
	private final MobileRobot robot;
	private final ValueMap valueMap;

	private boolean running;

	public MobileRobotAIgrid(MobileRobot robot, OccupancyMap map) {
		this.map = map;
		this.robot = robot;
		this.valueMap = new ValueMap(map);
	}

	/**
	 * In this method the gui.controller sends commands to the robot and its devices.
	 * At the moment all the commands are hardcoded.
	 * The exercise is to let the gui.controller make intelligent decisions based on
	 * what has been discovered so far. This information is contained in the OccupancyMap.
	 */
	public void run() {
		String result;
		this.running = true;
		double position[] = new double[3];
		double laserMeasures[] = new double[360];
		double sonarMeasures[] = new double[360];
		
		try {

			PipedInputStream pipeIn = new PipedInputStream();
			BufferedReader input = new BufferedReader(new InputStreamReader(pipeIn));
			PrintWriter output = new PrintWriter(new PipedOutputStream(pipeIn), true);

			robot.setOutput(output);
			System.out.println("intelligence running");
			
//      ases where a variable value is never used after its assignment, i.e.:
			
			Integer x;
			Integer y;
			int rotation;
			
			boolean firstscan = true;
			
			while (running && (!isDone() || firstscan)) {
				firstscan = false;
				
				robot.sendCommand("R1.GETPOS");
				result = input.readLine();
				parsePosition(result, position);
				
				robot.sendCommand("L1.SCAN");
				result = input.readLine();
				parseMeasures(result, laserMeasures);
				
				robot.sendCommand("S1.SCAN");
				result = input.readLine();
				parseMeasures(result, sonarMeasures);
				
				map.drawLaserScan(position, combineMeasures(sonarMeasures, laserMeasures));
				
				x = (int)Math.ceil(position[0]/10);
				y = (int)Math.ceil(position[1]/10);
				rotation = (int)Math.ceil(position[2])/90;
				
				valueMap.generateMap(x,y,0);
				valueMap.printGrid();
				
				List<Integer> list = valueMap.generatePath();
				map.drawPath(list, x,y);
				
				int relative;
				for(int i : list)
				{
					relative = (rotation+i)%4;
					System.out.println("Command: " + i + ", rotation: " + rotation + ", Relative: " + relative);
					switch(relative)
					{
					case 0:
						rotation = ((rotation-1)+4)%4;
						robot.sendCommand("P1.ROTATELEFT 90");
						input.readLine();
						robot.sendCommand("P1.MOVEFW 10");
						input.readLine();
						
						break;
					case 1:
						robot.sendCommand("P1.MOVEBW 10");
						input.readLine();
						break;
					case 2:
						rotation = ((rotation+1)+4)%4;
						robot.sendCommand("P1.ROTATERIGHT 90");
						input.readLine();
						robot.sendCommand("P1.MOVEFW 10");
						input.readLine();
						
						break;
					case 3:
						robot.sendCommand("P1.MOVEFW 10");
						input.readLine();
						break;
					}
				}
				
				//running = false;
			}
			System.err.println("Nothing left to discover.");
			
			this.running = false;
		} catch (IOException ioe) {
			System.err.println("execution stopped");
			running = false;
		}

	}

	private int getNextMove(Integer x, Integer y)
	{
		if(y > 0)
			if(map.getGrid()[x][y-1] == 'r')
			{
				return 0;
			}
				

		if(x < valueMap.getWidth()-1)
			if(map.getGrid()[x+1][y] == 'r')
			{
				return 3;
			}
		
		if(y < valueMap.getHeight()-1)
			if(map.getGrid()[x][y+1] == 'r')
			{
				return 2;
			}
		
		if(x > 0)
			if(map.getGrid()[x-1][y] == 'r')
			{
				return 1;
			}
		
		return -1;
	}
	

	private void parsePosition(String value, double position[]) {
		int indexInit;
		int indexEnd;
		String parameter;

		indexInit = value.indexOf("X=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[0] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("Y=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[1] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("DIR=");
		parameter = value.substring(indexInit + 4);
		position[2] = Double.parseDouble(parameter);
	}
	
	private double[] combineMeasures(double m1[], double m2[])
	{
		double[] m = new double[360];
		for(int i = 0;i <360;i++)
			m[i] = m1[i] < m2[i] ? m1[i] : m2[i];
		return m;
	}

	private void parseMeasures(String value, double measures[]) {
		//System.out.print(value);
		for (int i = 0; i < 360; i++) {
			measures[i] = 100.0;
		}
		if (value.length() >= 5) {
			value = value.substring(5);  // removes the "SCAN " keyword

			StringTokenizer tokenizer = new StringTokenizer(value, " ");

			double distance;
			int direction;
			while (tokenizer.hasMoreTokens()) {
				distance = Double.parseDouble(tokenizer.nextToken().substring(2));
				direction = (int) Math.round(Math.toDegrees(Double.parseDouble(tokenizer.nextToken().substring(2))));
				if (direction == 360) {
					direction = 0;
				}
				measures[direction] = distance;
				// Printing out all the degrees and what it encountered.
				//System.out.println("direction = " + direction + " distance = " + distance);
			}
		}
	}
	
	private boolean isDone()
	{
		char[][] m =  map.getGrid();
		int height = m[0].length;
		int width = m.length;
		int fieldCount = height * width;
		char field;
		System.out.println(height + ":" + width);
		
		for(int i = 0;i < fieldCount; i++)
		{
			int fx = i%width;
			int fy = i/width;
			field = m[fx][fy];
			
			if(field == map.getEmpty())
			{
				if(fx != 0)
					if(m[fx-1][fy] == map.getUnknown()) return false;
				if(fx != width-1)
					if(m[fx+1][fy] == map.getUnknown()) return false;
				if(fy != 0)
					if(m[fx][fy-1] == map.getUnknown()) return false;
				if(fy != height-1)
					if(m[fx][fy+1] == map.getUnknown()) return false;
			}
		}
		
		return true;
	}
}
